/*
 * MART.cpp
 *
 *  Created on: Jun 15, 2011
 *      Author: fqhuy
 */
#include "MART.hpp"

#include <string>
#include "include/quad2d.h"
//#include "include/qromb2d.h"
#include "Utilities.hpp"
#include <cstdlib>
#include <iostream>

using namespace std;

MART::MART(float Pixelsize, float VPScaleFactor, float Start_VoxelX,
		float End_VoxelX, float Start_VoxelY, float End_VoxelY,
		float Start_VoxelZ, float End_VoxelZ, long PixelX, long PixelY,
		int Itr, double xc3d[MAX_NUM_CALIB_MARKERS],
		double yc3d[MAX_NUM_CALIB_MARKERS], double zc3d[MAX_NUM_CALIB_MARKERS],
		double **xcl, double **ycl, char ***images, int ncp, float mu, int log,
		int use_hint) :

	Pixelsize(Pixelsize), VPScaleFactor(VPScaleFactor), Start_VoxelX(
			Start_VoxelX), Start_VoxelY(Start_VoxelY), Start_VoxelZ(
			Start_VoxelZ), End_VoxelX(End_VoxelX), End_VoxelY(End_VoxelY),
			End_VoxelZ(End_VoxelZ), PixelX(PixelX), PixelY(PixelY), xcl(NULL),
			ycl(NULL), Images(NULL), NoofFullItr(Itr), NCP(ncp), Mu(mu), Log(log),
			Use_Hint(use_hint) {

	if (xc3d != NULL && yc3d != NULL && zc3d != NULL)
		for (int i = 1; i <= ncp; i++) {
			h1.xc3d[i] = xc3d[i];
			h1.yc3d[i] = yc3d[i];
			h1.zc3d[i] = zc3d[i];
			for (int j = 0; j <= Num_Camera; j++) {
				this->xcl[i][j] = xcl[i][j];
				this->ycl[i][j] = ycl[i][j];
			}
		}

	int Area = 40 * 40;
	this->Pixelsize = Area / (256.0 * 256.0);
	Voxel_size = VPScaleFactor * this->Pixelsize; // size of a voxel wrt to pixel  in mm

	//for entire volume
	VoxelX = (long) ((End_VoxelX - Start_VoxelX) / Voxel_size); //number of Voxel per volume in X direction
	VoxelY = (long) ((End_VoxelY - Start_VoxelY) / Voxel_size); //number of Voxel per volume in Y direction
	VoxelZ = (long) ((End_VoxelZ - Start_VoxelZ) / Voxel_size); //number of Voxel per volume in Z direction

	nz = VoxelZ;

	Intensity = Allocate3D<float> (VoxelX, VoxelY, VoxelZ);

	if (Use_Hint) {
		W = Allocate3D<float> (Use_Hint, nz, NUM_LOS_PIXELS);
	} else {
		W = Allocate3D<float> (PixelX * PixelY, nz, NUM_LOS_PIXELS);
	}

	if (Use_Hint) {
		Los_Voxels_Ind = Allocate3D<long> (Use_Hint + 1, nz + 1,
				NUM_LOS_PIXELS + 1);
	} else {
		Los_Voxels_Ind = Allocate3D<long> (PixelX * PixelY + 1, nz + 1,
				NUM_LOS_PIXELS + 1); // LOS voxels coordinates   // No of voxel in the LOS of pixel.Los_Voxels_Ind[PixelX*PixelX][9][VoxelZ][3];
	}
	//Image = Allocate2D<int> (PixelX, PixelY);

	Voxel_volume = pow(Voxel_size, 3); // volume of a cube voxel

	Sphr = pow(3.0 / 4.0 * M_PI * Voxel_volume, 1 / 3.0); // radius of sphere with volume equal to cube voxel

	Cylr = 1 * sqrt(this->Pixelsize * this->Pixelsize); // radius of cylinder to be estimated from pixel

	printf("VoxelX: %d, VoxelY: %d, VoxelZ: %d, Voxel_size: %f, Voxel_volume: %f, Sphr: %f, Cylr: %f\n",VoxelX, VoxelY,VoxelZ, Voxel_size, Voxel_volume, Sphr, Cylr);
}

//---------------------------------------------------------------------------
void MART::ChooseCamera() {
	for (int i = 1; i <= NCP; i++) {
		h1.xcl[i] = xcl[i][Current_Camera];
		h1.ycl[i] = ycl[i][Current_Camera];
	}
	Image = Images[Current_Camera];
}
//---------------------------------------------------------------------------
void MART::CalculateLOS(float x, float y, float Z, float *XY) {
	float **A, *B, *tempv;
	float c11, c12, c13, c14, c21, c22, c23, c24, c31, c32, c33;
	A = Allocate2D<float> (2, 2);
	B = (float*) malloc(2 * sizeof(float));

	c11 = C[1];
	c12 = C[2];
	c13 = C[3];
	c14 = C[4];
	c21 = C[5];
	c22 = C[6];
	c23 = C[7];
	c24 = C[8];
	c31 = C[9];
	c32 = C[10];
	c33 = C[11];

	A[0][0] = c11 - c31 * x;
	A[0][1] = c12 - c32 * x;
	B[0] = x - c13 * Z - c14 + c33 * Z * x;
	A[1][0] = c21 - c31 * y;
	A[1][1] = c22 - c32 * y;
	B[1] = y - c23 * Z - c24 + c33 * Z * y;

	solve_linear_eqn(A, B, 2, XY);
	Deallocate2D(A, 2);
	free(B);

}
//---------------------------------------------------------------------------
void MART::InitialiseWeight() {
	if (Current_Camera == 0 && Itr == 0) {
		// camera1   first iteration
		for (int i = 0; i < VoxelX; i++)
			for (int j = 0; j < VoxelY; j++)
				for (int k = 0; k < VoxelZ; k++)
					Intensity[i][j][k] = 1;
	} else { // camera2 ,3 ...
		int countI = 0;
		if ((fpIntensity = fopen("output/Volume.dat", "rt")) == NULL) {
			cerr << "Cannot open input file. Volume.dat";
		}
		for (int i = 0; i < VoxelX; i++)
			for (int j = 0; j < VoxelY; j++)
				for (int k = 0; k < VoxelZ; k++) {
					if (fscanf(fpIntensity, "%d %d %d %f %f %f %f", &Dumi,
							&Dumi, &Dumi, &Intensity[i][j][k], &Dum, &Dum, &Dum)
							== EOF)
						break;
					countI++;

				}

		fclose(fpIntensity);
		if (countI != VoxelX * VoxelY * VoxelZ) {
			cerr
					<< "The Intensity data file does not match with Voxel size!!!\n";
			return;
		}
	}
	//Voxel_Los=NUM_LOS_PIXELS*nz;
	double Intersection = 1;

	//temp parameters for coordinates
	float XY[3], xy[2], max[2] = { 0, 0 }, min[2] = { 2e32, 2e32 }; // coordinates for 2D to 3D conversion
	float LOS_VoxelXY[2]; // coordinates for LOS 3D X,Y points for each z-plane
	float intervalx, intervaly, intervalz; // interval for X,Y,Z variation (Assumming NUM_LOS_PIXELS voxel exits for each LOS)
	bool flagNo_LOS = false, flagNo_LOSY = false;
	int countIntensitychecked;

	std::string WeightFile;
	int stop1, stop2, i, j;

	if (Use_Hint) {
		stop1 = 1;
		stop2 = Use_Hint;//Num_Particles[Current_Camera];
	} else {
		stop1 = PixelX;
		stop2 = PixelY;
	}

	if (Log) {
		WeightFile += std::string("output/Weight_") + IntToStr(Itr)
				+ std::string("_") + IntToStr(Current_Camera + 1)
				+ std::string(".txt");

		fdata3d = fopen(WeightFile.c_str(), "wt");
		fprintf(fdata3d,
				" Index K L X Y Z flagNo_LOS Los_Voxels_Ind LOS_VoxelX LOS_VoxelY d W\n");
	}

	for (int ii = 0; ii < stop1; ii++)
		for (int jj = 0; jj < stop2; jj++) {
			if (Use_Hint) {
				i = Hint[Current_Camera][jj][0];
				j = Hint[Current_Camera][jj][1];
				Index = jj;
			} else {
				i = ii;
				j = jj;

				if (Image[i][j] == 0)
					continue;

				Index = i + j * PixelX;
			}



			xy[0] = i + 0.5;
			xy[1] = j + 0.5;
			intervalz = Start_VoxelZ;

			for (int k = 0; k < nz; k++) {
				XY[2] = (intervalz + Voxel_size / 2);

				// checking if  LOS is within observation volume
				if (XY[2] > End_VoxelZ) {
					flagNo_LOS = true;
				} else if (XY[2] < Start_VoxelZ) {
					flagNo_LOS = true;
				} else {
					CalculateLOS(xy[0], xy[1], XY[2], XY);// estimate LOS point on z-plane
					if (max[0] < XY[0]) {
						max[0] = XY[0];
					}
					if (max[1] < XY[1]) {
						max[1] = XY[1];
					}
					if (min[0] > XY[0]) {
						min[0] = XY[0];
					}
					if (min[1] > XY[1]) {
						min[1] = XY[1];
					}
					flagNo_LOS = false;

				}
				intervalx = -(int) (sqrt(NUM_LOS_PIXELS) / 2);
				countIntensitychecked = 0;

				///For each z-plane  the NUM_LOS_PIXELS voxels are encounter by a pixel LOS
				for (int m = 0; m < sqrt(NUM_LOS_PIXELS); m++) {
					intervaly = -(int) (sqrt(NUM_LOS_PIXELS) / 2);
					LOS_VoxelXY[0] = XY[0] + intervalx * Voxel_size;
					// checking if  LOS is within observation volume
					if (LOS_VoxelXY[0] > End_VoxelX) {
						flagNo_LOS = true;
					} else if (LOS_VoxelXY[0] < Start_VoxelX) {
						flagNo_LOS = true;
					} else {
						flagNo_LOS = false;
					}

					flagNo_LOSY = false;
					for (int n = 0; n < sqrt(NUM_LOS_PIXELS); n++) {
						long l = m + n * sqrt(NUM_LOS_PIXELS);
						LOS_VoxelXY[1] = XY[1] + intervaly * Voxel_size;

						if (flagNo_LOS) {
							Los_Voxels_Ind[Index][k][l] = VoxelX * VoxelY
									* VoxelZ;
							W[Index][k][l] = 0.0;
						} else {
							// checking if  LOS is within observation volume
							if (LOS_VoxelXY[1] > End_VoxelY) {
								flagNo_LOSY = true;
							} else if (LOS_VoxelXY[1] < Start_VoxelY) {
								flagNo_LOSY = true;
							} else {
								//to be calculate from pixel line of sight

								Dcsphacyl = sqrt((LOS_VoxelXY[0] - XY[0])
										* (LOS_VoxelXY[0] - XY[0])
										+ (LOS_VoxelXY[1] - XY[1])
												* (LOS_VoxelXY[1] - XY[1]));

								// Condition if and only if d<=r+row
								if (Dcsphacyl > (Sphr + Cylr)) {
									flagNo_LOSY = true;

								} else {
									//Index estimation for variable voxel case


									Index3d = (long) ((int) ((LOS_VoxelXY[0]
											- Start_VoxelX) / Voxel_size)
											+ (int) ((LOS_VoxelXY[1]
													- Start_VoxelY)
													/ Voxel_size) * (VoxelX)
											+ k * (VoxelX) * (VoxelY));

									// calculating the volume of intersection of sphere and cylinder
									/*
									 Ftor func(Sphr);
									 Fty1 lymin;
									 Fty2 lymax(Cylr, Sphr, Dcsphacyl);

									 Intersection = 4 * quad2d_t(func, GetMax(
									 Dcsphacyl - Cylr, -Sphr), GetMin(
									 Dcsphacyl + Cylr, Sphr), lymin,
									 lymax);
									 */
									//Intersection=intersection();
									cylr = Cylr;
									sphr = Sphr;
									dcsphacyl = Dcsphacyl;

									Intersection = 4 * quad2d(func, GetMax(
											Dcsphacyl - Cylr, -Sphr), GetMin(
											Dcsphacyl + Cylr, Sphr), lymin,
											lymax);

									W[Index][k][l] = Intersection
											/ Voxel_volume;

									if (W[Index][k][l] > 5)
										cerr << "Warning: W = "<< W[Index][k][l] <<" > 5\n";

									Los_Voxels_Ind[Index][k][l] = Index3d;

									//cout << Index3d << " ";
									flagNo_LOSY = false;
								}

							}

							if (flagNo_LOSY) {
								countIntensitychecked++;

								Los_Voxels_Ind[Index][k][l] = VoxelX * VoxelY
										* VoxelZ;
								W[Index][k][l] = 0.0;
								flagNo_LOSY = false;
							}
						}
						if (Log)
							fprintf(
									fdata3d,
									"%ld %d %d %0.8f %0.8f %0.8f %s %ld %0.8f %0.8f %0.8f %0.8f\n",
									Index, k, l, XY[0], XY[1], XY[2],
									flagNo_LOS ? "TRUE" : "FALSE",
									Los_Voxels_Ind[Index][k][l],
									LOS_VoxelXY[0], LOS_VoxelXY[1], Dcsphacyl,
									W[Index][k][l]);
						intervaly++;

					}// end of n
					intervalx++;
				}// end of m
				intervalz += Voxel_size;
			}// end of Z
		}

	if (Log)
		fclose(fdata3d);
}
//---------------------------------------------------------------------------
void MART::MARTUpdate() {
	float sum = 0, factor = 1, max = 1e32;
	int mew = 2, stop1, stop2, i, j;

	if (Use_Hint) {
		stop1 = 1;
		stop2 = Use_Hint;//Num_Particles[Current_Camera];
	} else {
		stop1 = PixelX;
		stop2 = PixelY;
	}
	//FILE* fac = fopen("factor.txt","wt");
	for (int ii = 0; ii < stop1; ii++)
		for (int jj = 0; jj < stop2; jj++) {

			if (Use_Hint) {
				i = Hint[Current_Camera][jj][0];
				j = Hint[Current_Camera][jj][1];
				Index = jj;
			} else {
				i = ii;
				j = jj;
				if (Image[i][j] == 0)
					continue;
				Index = i + j * PixelX;
			}
			//First calculate the sum of W*E(X,Y,Z) along the line of sight.
			sum = 0;


			int tempv[3];
			for (int k = 0; k < VoxelZ; k++)
				for (int l = 0; l < NUM_LOS_PIXELS; l++) {
					indexcheck = Los_Voxels_Ind[Index][k][l];

					if (indexcheck < VoxelX * VoxelY * VoxelZ) {
						Decodexyz(Los_Voxels_Ind[Index][k][l], VoxelX, VoxelY,
								VoxelZ, tempv);

						sum += W[Index][k][l]
								* Intensity[tempv[0]][tempv[1]][tempv[2]];

					}
				}

			//Keep the max sum for checking purpose.
			if (max < sum) {
				max = sum;
			}

			//Update the intensity using the pre-calculated sum.
			if (sum != 0) {
				factor = Image[i][j] / sum;

				for (int k = 0; k < VoxelZ; k++)
					for (int l = 0; l < NUM_LOS_PIXELS; l++) {
						//If the voxel index is inside the volume.
						if (Los_Voxels_Ind[Index][k][l] < VoxelX * VoxelY
								* VoxelZ) {

							Decodexyz(Los_Voxels_Ind[Index][k][l], VoxelX,
									VoxelY, VoxelZ, tempv);

							if (W[Index][k][l] != 0 && factor != 0) {
								//Intensity[tempv[0]][tempv[1]][tempv[2]]
								double temp =
										Intensity[tempv[0]][tempv[1]][tempv[2]]
												* pow(factor, fabs(Mu
														* W[Index][k][l]));

								if (temp > 10000 || W[Index][k][l] > 1)
									cout << "Warning: Intensity = " << temp
											<< "Weight = " << W[Index][k][l]
											<< "\n";

								Intensity[tempv[0]][tempv[1]][tempv[2]] = temp;
								//fprintf(fac,"%d %d %f %f %f %f\n", k, l, temp, (double) W[Index][k][l], factor, sum);
							}
						}
					}
			}
		}
	//fclose(fac);
	WriteArray3D("Volume.dat", Intensity, VoxelX, VoxelY, VoxelZ, true);

	if (Log) {
		std::string backUp = std::string("Volume_") + IntToStr(Itr)
				+ std::string("_") + IntToStr(Current_Camera + 1)
				+ std::string(".dat");
		WriteArray3D(backUp.c_str(), Intensity, VoxelX, VoxelY, VoxelZ, true);
	}
}

void MART::StepRun(int j) {

	Current_Camera = j;
	Itr = 0;
	// Choosing image and camera
	ChooseCamera();

	// Calibration
	h1.calculate_parameters();

	// Initialise_Weight
	InitialiseWeight();

	// Update Intensity
	MARTUpdate();
}

void MART::FullRun() {
	//std::string command_subscript;
	double time;

	for (int i = 0; i < NoofFullItr; i++) {
		cout << "Iteration " << i << "\n";
		for (int j = 0; j < Num_Camera; j++) {
			// camera parameter
			Current_Camera = j;
			Itr = i;
			// Choosing image and camera
			cout << "...Camera - " << j << "\n";
			ChooseCamera();
			// Calibration
			h1.calculate_parameters();
			// Initialise_Weight
			Timer::deltaT(0);
			InitialiseWeight();
			time = Timer::deltaT(0);
			std::cout << "......Initialized weight matrix. Elapsed time is "
					<< time << "\n";

			if (Log) {
				WriteArray3D("Los_Voxels_Ind.txt", Los_Voxels_Ind, PixelX
						* PixelY + 1, nz + 1, NUM_LOS_PIXELS + 1);
				WriteArray3D("W.txt", Los_Voxels_Ind, PixelX * PixelY + 1, nz
						+ 1, NUM_LOS_PIXELS + 1);
			}
			// Update Intensity
			Timer::deltaT(0);
			MARTUpdate();
			time = Timer::deltaT(0);
			std::cout << "......Updated intensity matrix. Elapsed time is "
					<< time << "\n";

			if (Log)
				WriteArray3D("Intensity.txt", Intensity, 10, 10, 6);
		}

	}
	MeanFilter();
	/*
	 * if (CheckBox3->Checked) {
	 mean_filter();
	 std::string VolumeFile;
	 VolumeFile="Volume_MeanFilter.dat";

	 if ((fpIntensity=fopen(VolumeFile.t_str(),"wt"))== NULL)  {
	 ShowMessage("Cannot open input file.\n");  }
	 for(int i = 0; i < VoxelX; i++ )
	 for(int j = 0; j < VoxelY; j++ )
	 for(int k = 0; k <VoxelZ; k++ )
	 fprintf(fp1,"%d %d %d %0.9f %0.9f %0.9f %0.9f\n",i,j,k,Intensity[i][j][k],(Start_VoxelX+i*Voxel_size),(Start_VoxelY+j*Voxel_size),(Start_VoxelZ+k*Voxel_size));
	 fprintf(fp1,"0 0 0 %0.9f 0 0 0\n",Voxel_size);
	 fclose(fpIntensity);
	 }
	 */
}
//---------------------------------------------------------------------------
void MART::MeanFilter() {
	float sum;
	for (int i = 0; i < VoxelX; i++)
		for (int j = 0; j < VoxelY; j++)
			for (int k = 0; k < VoxelZ; k++) {
				sum = 0;
				for (int l = -1; l < 2; l++)
					for (int m = -1; m < 2; m++)
						for (int n = -1; n < 2; n++) {

							if ((i + l) >= 0 && (i + l) < VoxelX && (j + m)
									>= 0 && (j + m) < VoxelY && (k + n) >= 0
									&& (k + n) < VoxelZ) {
								sum += Intensity[i + l][j + m][k + n];

							}
						}
				Intensity[i][j][k] = sum / 27;

			}

}
//---------------------------------------------------------------------------
void MART::Decodexyz(long index, int maxx, int maxy, int maxz, int* a) // 3D/2D indices to linear index
{

	long tempindex = 0;
	for (int i = 0; i < maxx; i++)
		for (int j = 0; j < maxy; j++)
			for (int k = 0; k < maxz; k++) {

				tempindex = i + j * maxx + k * maxx * maxy;
				if (tempindex == index) { //TODO: fix this
					a[0] = i;
					a[1] = j;
					a[2] = k;
					return;

				}
			}
}
